ROS Twist和Odometry消息类型使用（Python）

#ROS Twist和Odometry消息类型使用（Python）| 来源: 网络整理| 查看: 265

消息类型：

1. Twist - 线速度角速度

通常被用于发送到/cmd_vel话题，被base controller节点监听，控制机器人运动

geometry_msgs/Twist geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z

linear.x指向机器人前方，linear.y指向左方，linear.z垂直向上满足右手系，平面移动机器人常常linear.y和linear.z均为0

angular.z代表平面机器人的角速度，因为此时z轴为旋转轴

示例：

#! /usr/bin/env python ''' Author: xushangnjlh at gmail dot com Date: 2017/05/22 @package forward_and_back ''' import rospy from geometry_msgs.msg import Twist from math import pi class ForwardAndBack(): def __init__(self): rospy.init_node('forward_and_back', anonymous=False) rospy.on_shutdown(self.shutdown) # this "forward_and_back" node will publish Twist type msgs to /cmd_vel # topic, where this node act like a Publisher self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) # parameters rate = 50 r = rospy.Rate(rate) linear_speed = 0.2 goal_distance = 5 linear_duration = goal_distance/linear_speed angular_speed = 1.0 goal_angular = pi angular_duration = goal_angular/angular_speed # forward->rotate->back->rotate for i in range(2): # this is the msgs variant, has Twist type, no data now move_cmd = Twist() move_cmd.linear.x = linear_speed # # should keep publishing ticks = int(linear_duration*rate) for t in range(ticks): # one node can publish msgs to different topics, here only publish # to /cmd_vel self.cmd_vel.publish(move_cmd) r.sleep # sleep according to the rate # stop 1 ms before ratate move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1) move_cmd.angular_speed.z = angular_speed ticks = int(angular_duration*rate) for t in range(ticks): self.cmd_vel.publish(move_cmd) r.sleep() self.cmd_vel.publish(Twist()) rospy.sleep(1) def shutdown(self): rospy.loginfo("Stopping the robot") self.cmd_vel.publish(Twist()) rospy.sleep(1) if __name__=='__main__': try: ForwardAndBack() except: rospy.loginfo("forward_and_back node terminated by exception") 2. nav_msgs/Odometry - 里程计（位姿+线速度角速度+各自协方差）

通常，发布到/cmd_vel topic然后被机器人（例如/base_controller节点）监听到的Twist消息是不可能准确地转换为实际的机器人运动路径的，误差来源于机器人内部传感器误差（编码器），标定精度，以及环境影响（地面是否打滑平整）；因此我们可以用更准确的Odometry消息类型类获取机器人位姿（/odom到/base_link的tf变换）。在标定准确时，该消息类型与真实的机器人位姿误差大致在1%量级（即使在rviz中显示的依然误差较大）。

还包括

参考系信息，Odometry使用/odom作为parent frame id，/base_link作为child frame id；也就是说世界参考系为/odom（通常设定为起始位姿，固定不动），移动参考系为/base_link（这里还有点不理解，后面来填坑）时间戳，因此不仅知道运动轨迹，还可以知道对应时间点 Header header string child_frame_id geometry_msgs/PoseWithCovariance pose geometry_msgs/TwistWithCovariance twist # 展开 Header header uint32 seq time stamp string frame_id string child_frame_id geometry_msgs/PoseWithCovariance pose geometry_msgs/Pose pose geometry_msgs/Point position float64 x float64 y float64 z geometry_msgs/Quaternion orientation float64 x float64 y float64 z float64 w float64[36] covariance geometry_msgs/TwistWithCovariance twist geometry_msgs/Twist twist geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z float64[36] covariance

示例：

运动路径和位姿通过内部的Odometry获取，该Odemetry的位姿通过监听tf坐标系变换获取（/odom和/base_link）

#! /usr/bin/env python ''' Author: xushangnjlh at gmail dot com Date: 2017/05/23 @package odometry_forward_and_back ''' import rospy from geometry_msgs.msg import Twist, Point, Quaternion import tf from rbx1_nav.tranform_utils import quat_to_angle, normalize_angle from math import pi, radians, copysign, sqrt, pow class Odometry_forward_and_back: def __init__(self): rospy.ini_node('odometry_forward_and_back', anonymous=False) rospy.on_shutdown(self.shutdown) self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) rate = 20 r = rospy.Rate(rate) linear_speed = 0.2 goal_distance =1.0 angular_speed = 1.0 goal_angle = pi angular_tolerance = radians(2.5) # Initialize tf listener, and give some time to fill its buffer self.tf_listener = tf.TransformListener() rospy.sleep(2) # Set odom_frame and base_frame self.odom_frame = '/odom' try: self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_footprint' except(tf.Exception, tf.ConnectivityException, tf.LookupException): try: self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_link' except(tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("Cannot find base_frame transformed from /odom") rospy.signal_shutdown("tf Exception") position = Point() for i in range(2): move_cmd = Twist() move_cmd.linear.x = linear_speed # Initial pose, obtained from internal odometry (position, rotation) = self.get_odom() x_start = position.x y_start = position.y distance = 0 # Keep publishing Twist msgs, until the internal odometry reach the goal while distance < goal_distance and not rospy.is_shutdown(): self.cmd_vel.publish(move_cmd) r.sleep() (position, rotation) = self.get_odom() distance = sqrt( pow( (position.x-x_start), 2 ) + \ pow( (position.y-y_start), 2 ) ) # Stop 1 ms before rotate move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1) move_cmd.angular.z = angular_speed # should be the current ration from odom angle_last = rotation angle_turn = 0 while abs(angle_turn+angular_tolerance)

【本文地址】

公司简介

联系我们